
/*
* core one, cooperator.
* proc message.
*/

#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>

#include "rp2040.h"
#include "hardware/gpio.h"
#include "hardware/i2c.h"
#include "pico.h"
#include "pico/multicore.h"

#include "umsg.h"

/*
* 0 = data[0],  init : [1] [2] [3] [4] = baud
* 1 = data[0],  read : [1] = addr, [2] = cnt
* 2 = data[0], write : [1] = addr, [...] = data
* 3 = data[0], general call : write..
*/
void p1_test_i2c( umsg_t * pmsg )
{
	i2c_inst_t * pi2c = i2c0;
	uint8_t * ptr = pmsg->data;
	uint32_t temp;
	uint8_t addr;
	int tlen;
	int iret;
	int i;
	uint8_t tary[32];
	uint8_t repy[4];


	switch ( ptr[0] ) {
	
	case 0:
		temp = ptr[4];
		temp = (temp << 8) | ptr[3];
		temp = (temp << 8) | ptr[2];
		temp = (temp << 8) | ptr[1];
		printf("i2c init, %u\n", temp );
		i2c_init( pi2c, temp );
		break;

	case 1:
		addr = ptr[1];
		tlen = ptr[2];
		iret = i2c_read_blocking( pi2c, addr, tary, tlen, false );

		printf("read = %d\n", iret );
		
		if ( iret > 0 ) {
			int  i;
			for ( i=0; i<iret; i++ ) {
				printf( "%x : ", tary[i] );
			}
			printf( "\n\n");
		}

		break;

	case 2:
		addr = ptr[1];
		tlen = pmsg->mlen - 2;
		for ( i=0; i<tlen; i++ ) {
			tary[i] = ptr[i + 2];
		}

		iret = i2c_write_blocking( pi2c, addr, tary, tlen, false );
		if ( iret != tlen ) {
			printf( "write, ret = %d\n", iret );
			break;
		}

		printf( "write, succ\n" );
		break;

	case 3:
		/* process call, write, restart, read */
		addr = ptr[1];
		tlen = pmsg->mlen - 3;
		for ( i=0; i<tlen; i++ ) {
			tary[i] = ptr[i + 3];
		}

		repy[0] = (uint8_t)tlen;
		iret = i2c_write_blocking( pi2c, addr, tary, tlen, true );
		if ( iret != tlen ) {
			printf( "write, ret = %d\n", iret );
			break;
		}

		tlen = ptr[2];
		repy[1] = (uint8_t)tlen;
		iret = i2c_read_blocking( pi2c, addr, tary, tlen, false );
		if ( iret != tlen ) {
			printf( "read, ret = %d\n", iret );
			break;
		}

		repy[2] = 0x55;
		repy[3] = 0x66;
		break;
	
	case 4:
		/* StartByte, master send  */

		addr = ptr[1];
		tlen = pmsg->mlen - 2;
		for ( i=0; i<tlen; i++ ) {
			tary[i] = ptr[i + 2];
		}

		pi2c->hw->enable = 0;
		temp = I2C_IC_TAR_SPECIAL_VALUE_ENABLED << I2C_IC_TAR_SPECIAL_LSB;
		temp = temp | (I2C_IC_TAR_GC_OR_START_VALUE_START_BYTE << I2C_IC_TAR_GC_OR_START_LSB);
		temp = temp | addr;
		pi2c->hw->tar = temp;
		pi2c->hw->enable = 1;

		for ( i=0; i<tlen; i++ ) {

			temp = I2C_IC_DATA_CMD_CMD_VALUE_WRITE << I2C_IC_DATA_CMD_CMD_LSB;
			temp = temp | tary[i];

			if ( (i + 1) == tlen ) {
				temp = temp | I2C_IC_DATA_CMD_STOP_VALUE_ENABLE << I2C_IC_DATA_CMD_STOP_LSB;
			}
			
			pi2c->hw->data_cmd = temp;
		}

		/* check and wait */
		while ( true ) {

			temp = pi2c->hw->raw_intr_stat;

            if ( temp & I2C_IC_RAW_INTR_STAT_TX_ABRT_BITS ) {
            	iret = 0x11;
                break;
            }

            if ( temp & I2C_IC_RAW_INTR_STAT_STOP_DET_BITS ) {
            	iret = 0;
                break;
            }

#if 0
            if ( (pi2c->hw->status & 0x1) == 0 ) {
            	iret = 0;
                break;
            }
#endif

		}

        temp = pi2c->hw->tx_abrt_source;
        if ( temp != 0 ) {
            // Note clearing the abort flag also clears the reason, and
            // this instance of flag is clear-on-read! Note also the
            // IC_CLR_TX_ABRT register always reads as 0.
            pi2c->hw->clr_tx_abrt;
            pi2c->hw->clr_intr;
		}
        
        repy[0] = temp & 0xFF;
        repy[1] = (temp >> 8) & 0xFF;
        repy[2] = (temp >> 16) & 0xFF;
        repy[3] = (temp >> 24) & 0xFF;

        /**/
        pi2c->hw->enable = 0;
        break;

	default:
		iret = -1;
		break;
	}

	pmsg->type = 0x10;
	pmsg->mlen = 5;
	pmsg->data[0] = (uint8_t)iret;
	pmsg->data[1] = repy[0];
	pmsg->data[2] = repy[1];
	pmsg->data[3] = repy[2];
	pmsg->data[4] = repy[3];
	multicore_fifo_push_blocking( (intptr_t)pmsg );
	return;
}


#include "hardware/resets.h"
#include "hardware/pio.h"
#include "hardware/pio_instructions.h"


/**/
static const uint16_t ws2812_program_instructions[] = {
            //     .wrap_target
    0x80a0, //  0: pull   block
    0xe037, //  1: set    x, 24
    0xe301, //  2: set    pins, 1                [3]
    0x6401, //  3: out    pins, 1                [3]
    0xe200, //  4: set    pins, 0                [2]
    0x0042, //  5: jmp    x--, 2
    0x0000, //  6: jmp    0
            //     .wrap
};

static const struct pio_program ws2812_program = {
    .instructions = ws2812_program_instructions,
    .length = 7,
    .origin = -1,
};




int p1_test_pio( umsg_t * pmsg )
{
	PIO pio = pio0;
	uint sm = 0;
	uint opin = 16;
	pio_sm_config cfg;

	/* unreset */
	unreset_block_wait( RESETS_RESET_PIO0_BITS );

	/* gpio */
	pio_gpio_init( pio, opin );
	pio_sm_set_consecutive_pindirs(pio, sm, opin, 1, true );

	/* config */
	cfg = pio_get_default_sm_config();
	sm_config_set_out_pins( &cfg, opin, 1 );
	sm_config_set_set_pins( &cfg, opin, 1 );
	sm_config_set_in_pins( &cfg, opin );
	sm_config_set_sideset_pins( &cfg, opin );
	sm_config_set_sideset( &cfg, 0, 0, false );
	sm_config_set_clkdiv( &cfg, 12.5 );
	
	/* INST_MEM */
	pio_add_program_at_offset( pio, &ws2812_program, 0 );

	/**/
	pio_sm_init(pio, sm, 0, &cfg );
	pio_sm_set_enabled( pio, sm, true );

	/**/
	pmsg->type = 0x12;
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );
	return 0;
}


int p1_test_pio_data( umsg_t * pmsg )
{
	PIO pio = pio0;
	int sm = 0;	
	uint32_t temp;

	/**/
	temp = *(uint32_t *)pmsg->data;
	temp = __RBIT(temp) >> 8;
	pio_sm_put_blocking( pio, sm, temp );

	/**/
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );	
	return 0;
}


/*
从 rp2040 角度看来, read 和 write 都是从输出若干 bit 开始的.
所以写入到 FIFO 

对于 read 操作, 只需要支持 32 位的 dr 寄存器.
因为从 had 模块的 verilog 代码看来, 对于 8 位的 ir 寄存器, read 操作仅仅返回全 1 ???

在 read 动作之前, 需要发送 start,rw,rs 总共 4 个 bits, 这个可以通过 write 操作完成.
如果在之前有 write ir 寄存器, 可以把这 4 个 bits 合并在一起完成.

read 的结果通过 FIFO - output 返回给用户程序, 一共是 2 个 word .
word-0 是读操作采集的 32 bits 寄存器内容.
word-1 只有最低 2 bits 有效, 
bit-0 是采集到的 odd parity, 可以用来判断是否正常.
bit-1 是对应时序中的 SYNC 周期的值, 按照协议应当是 低电平 0 .
这两个 bit 可以用于检查. 对应错误场景是 dut 断电? 线路短路等?

*/

#define had3_offset_read_stage 10u
#define had3_offset_finish_nty 25u

static const uint16_t had3_program_instructions[] = {
            //     .wrap_target
    0x80a0, //  0: pull   block
    0x7201, //  1: out    pins, 1         side 0 [2]
    0x6025, //  2: out    x, 5
    0x6045, //  3: out    y, 5
    0x1929, //  4: jmp    !x, 9           side 1 [1]
    0x0046, //  5: jmp    x--, 6
    0xa142, //  6: nop                           [1]
    0x7401, //  7: out    pins, 1         side 0 [4]
    0x1a46, //  8: jmp    x--, 6          side 1 [2]
    0xa0a2, //  9: mov    pc, y
    
    0xa042, // 10: nop
    0xf480, // 11: set    pindirs, 0      side 0 [4]
    0xfc01, // 12: set    pins, 1         side 1 [4]
    0xf43f, // 13: set    x, 31           side 0 [4]
    0x5a01, // 14: in     pins, 1         side 1 [2]
    0xa056, // 15: mov    y, ::isr
    0xa042, // 16: nop
    0xb442, // 17: nop                    side 0 [4]
    0x5a01, // 18: in     pins, 1         side 1 [2]
    0x0050, // 19: jmp    x--, 16
    0x8020, // 20: push   block
    0xb442, // 21: nop                    side 0 [4]
    0x5c01, // 22: in     pins, 1         side 1 [4]
    0xf481, // 23: set    pindirs, 1      side 0 [4]
    0x5c5f, // 24: in     y, 31           side 1 [4]

    0x8020, // 25: push   block
    0x0000, // 26: jmp    0
            //     .wrap
};


static const struct pio_program had3_program = {
    .instructions = had3_program_instructions,
    .length = 27,
    .origin = -1,
};


int had_init_pio( umsg_t * pmsg )
{
	PIO pio = pio0;
	uint sm = 0;
	uint clk_pin = 8;
	uint tms_pin = 9;
	pio_sm_config cfg;

	/* unreset */
	unreset_block_wait( RESETS_RESET_PIO0_BITS );

	/* gpio */
	gpio_pull_up( clk_pin );
	gpio_pull_up( tms_pin );
	pio_gpio_init( pio, clk_pin );
	pio_gpio_init( pio, tms_pin );
	pio_sm_set_consecutive_pindirs(pio, sm, clk_pin, 1, true );
	pio_sm_set_consecutive_pindirs(pio, sm, tms_pin, 1, true );
	pio_sm_set_pins( pio, sm, (1 << clk_pin) | (1 <<tms_pin) );

	/* config */
	cfg = pio_get_default_sm_config();
	sm_config_set_out_pins( &cfg, tms_pin, 1 );
	sm_config_set_set_pins( &cfg, tms_pin, 1 );
	sm_config_set_in_pins( &cfg, tms_pin );
	sm_config_set_sideset_pins( &cfg, clk_pin );
	sm_config_set_sideset( &cfg, 2, true, false );
	sm_config_set_clkdiv( &cfg, 125 );

	/* INST_MEM */
	pio_add_program_at_offset( pio, &had3_program, 0 );

	/**/
	pio_sm_init( pio, sm, 0, &cfg );
	pio_sm_set_enabled( pio, sm, true );

	/**/
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );	
	return 0;
}

/*
 cnt : 支持范围 1-22.
bits : 要输出的 bit 序列, 从最低 bit-0 开始串行输出.
 nxt : 执行之后跳转的 pc 值, 支持 3 个值, 0, 10, 25;
*/
static uint32_t had_encode_write( uint cnt, uint32_t bits, uint nxt )
{
	uint32_t t0;

	t0 = bits & 0x1;
	bits = bits >> 1;
	bits = (bits << 5) | nxt;
	bits = (bits << 5) | (cnt - 1);
	bits = (bits << 1) | t0;

	return bits;
}


int had_null_clock( void )
{
	PIO pio = pio0;
	int sm = 0;	
	uint32_t temp;
	uint32_t bit0;

	/**/
	temp = had_encode_write( 22, 0x3FFFFF, 0 );
	pio_sm_put_blocking( pio, sm, temp );
	
	temp = had_encode_write( 22, 0x3FFFFF, 0 );
	pio_sm_put_blocking( pio, sm, temp );

	temp = had_encode_write( 22, 0x3FFFFF, 0 );
	pio_sm_put_blocking( pio, sm, temp );

	temp = had_encode_write( 22, 0x3FFFFF, had3_offset_finish_nty );
	pio_sm_put_blocking( pio, sm, temp );

	/* wait finish */
	pio_sm_get_blocking( pio, sm );
	return 0;
}



static int had_write_ir8( uint8_t vir )
{
	PIO pio = pio0;
	int sm = 0;		
	uint32_t bits;
	uint32_t pary;

	pary = __builtin_popcount( vir ) + 1;
	bits = 0x2 | (pary & 0x1);
	bits = (bits << 8) | vir;
	bits = (bits << 5) | 0x18;			/* trn=1, rs=10, rw=0, st=0 : 11000 */

	/**/
	bits = had_encode_write( 15, bits, had3_offset_finish_nty );
	pio_sm_put_blocking( pio, sm, bits );

	/**/
	pio_sm_get_blocking( pio, sm );
	return 0;
}


static int had_write_dr32( uint32_t vdr )
{
	PIO pio = pio0;
	int sm = 0;		
	uint32_t bits;
	uint32_t pary;

	pary = __builtin_popcount( vdr ) + 1;
	pary = 0x2 | (pary & 0x1);
	pary = (pary << 16) | (vdr >> 16);

	bits = vdr & 0xFFFF;
	bits = (bits << 5) | 0x1C;		/* trn=1, rs=11, rw=0, st=0 : 11100 */

	bits = had_encode_write( 21, bits, 0 );
	pio_sm_put_blocking( pio, sm, bits );

	pary = had_encode_write( 18, pary, had3_offset_finish_nty );
	pio_sm_put_blocking( pio, sm, pary );

	/**/
	pio_sm_get_blocking( pio, sm );
	return 0;
}


static int had_read_dr32( uint32_t *pvdr )
{
	PIO pio = pio0;
	int sm = 0;		
	uint32_t bits;
	uint32_t pary;

	/* rs=11, rw=1, st=0 : 1110 */
	bits = had_encode_write( 4, 0xE, had3_offset_read_stage );
	pio_sm_put_blocking( pio, sm, bits );

	/**/
	bits = pio_sm_get_blocking( pio, sm );
	pary = pio_sm_get_blocking( pio, sm );

	/* check parity */
	if ( ( (__builtin_popcount(bits) + 1) & 0x01 ) != pary ) {
		return (int)(pary + 0x100);
	}

	/* ok, end */
	*pvdr = bits;
	return 0;
}


static int had_write_dr32_by_index( uint8_t vir, uint32_t vdr )
{
	/**/
	PIO pio = pio0;
	int sm = 0;
	uint32_t bits;
	uint32_t pary;

	/**/
	pary = __builtin_popcount( vir ) + 1;
	bits = 0x2 | (pary & 0x1);
	bits = (bits << 8) | vir;
	bits = (bits << 5) | 0x18;			/* trn=1, rs=10, rw=0, st=0 : 11000 */
	bits = had_encode_write( 15, bits, 0 );
	pio_sm_put_blocking( pio, sm, bits );

	/**/
	bits = vdr & 0xFFFF;
	bits = (bits << 5) | 0x1C;		/* trn=1, rs=11, rw=0, st=0 : 11100 */
	bits = had_encode_write( 21, bits, 0 );
	pio_sm_put_blocking( pio, sm, bits );

	/**/
	pary = __builtin_popcount( vdr ) + 1;
	pary = 0x2 | (pary & 0x1);
	pary = (pary << 16) | (vdr >> 16);
	pary = had_encode_write( 18, pary, had3_offset_finish_nty );
	pio_sm_put_blocking( pio, sm, pary );	

	/**/
	pio_sm_get_blocking( pio, sm );
	return 0;
}


static int had_read_dr32_by_index( uint8_t vir, uint32_t *pvdr )
{
	/**/
	PIO pio = pio0;
	int sm = 0;
	uint32_t bits;
	uint32_t pary;

	/* rs=11, rw=1, st=0 : 1110 */
	bits = 0xE;
	pary = __builtin_popcount( vir ) + 1;
	bits = (bits << 2) | 0x2 | (pary & 0x1);
	bits = (bits << 8) | vir;

	/* trn=1, rs=10, rw=0, st=0 : 11000 */
	bits = (bits << 5) | 0x18;			
	bits = had_encode_write( 19, bits, had3_offset_read_stage );
	pio_sm_put_blocking( pio, sm, bits );

	/**/
	bits = pio_sm_get_blocking( pio, sm );
	pary = pio_sm_get_blocking( pio, sm );

	/* check parity */
	if ( ( (__builtin_popcount(bits) + 1) & 0x01 ) != pary ) {
		return (int)(pary + 0x100);
	}

	/* ok, end */
	*pvdr = bits;
	return 0;
}


int had_msg_null_clock( umsg_t * pmsg )
{
	int iret;

	/**/
	iret = had_null_clock();
	*(uint32_t *)pmsg->data = (uint32_t)iret;
	pmsg->mlen = 4;


	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}


int had_msg_write_ir8( umsg_t * pmsg )
{
	int iret;

	/**/
	iret = had_write_ir8( pmsg->data[0] );

	/**/
	*(uint32_t *)pmsg->data = (uint32_t)iret;
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}


int had_msg_write_dr32( umsg_t * pmsg )
{
	int iret;
	uint32_t temp;

	/**/
	temp = *(uint32_t *)pmsg->data;
	iret = had_write_dr32( temp );
	
	/**/
	*(uint32_t *)pmsg->data = (uint32_t)iret;
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}


int had_msg_read_dr32( umsg_t * pmsg )
{
	int iret;
	uint32_t temp;

	/**/
	iret = had_read_dr32( &temp );
	
	/**/
	*(uint32_t *)pmsg->data = (uint32_t)iret;
	*(uint32_t *)(pmsg->data + 4) = temp;
	pmsg->mlen = 8;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}


int had_msg_write_dr32_by_index( umsg_t * pmsg )
{
	int iret;
	uint32_t temp;

	/**/
	temp = pmsg->data[4];
	temp = (temp << 8) | pmsg->data[3];
	temp = (temp << 8) | pmsg->data[2];
	temp = (temp << 8) | pmsg->data[1];

	/**/
	iret = had_write_dr32_by_index( pmsg->data[0], temp );

	/**/
	*(uint32_t *)pmsg->data = (uint32_t)iret;
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}


int had_msg_read_dr32_by_index( umsg_t * pmsg )
{
	int iret;
	uint32_t temp;

	/**/
	iret = had_read_dr32_by_index( pmsg->data[0], &temp );

	/**/
	*(uint32_t *)pmsg->data = (uint32_t)iret;
	*(uint32_t *)(pmsg->data + 4) = temp;
	pmsg->mlen = 8;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}


#define HREG_ID  		0x02
#define HREG_BYPASS  	0x0C
#define HREG_HCR	  	0x0D
#define HREG_HSR	  	0x0E
#define HREG_EHSR	  	0x0F
#define HREG_WBBR	  	0x11
#define HREG_PSR	  	0x12
#define HREG_PC	  		0x13
#define HREG_IR	  		0x14
#define HREG_CSR  		0x15
#define HREG_DDER  		0x16
#define HREG_ADDR 		0x18
#define HREG_DATA 		0x19
#define HREG_BSEL 		0x1E



/*
* 发送 80 个时钟信号, 保持 tms = 1 .
* 尝试读取 ID 寄存器, 判断通信是否正常.
* 判断 cpu 当前模式, 尝试进入 debug 状态, 需要 timeout 功能避免无穷等待.
* 如果上面判断都满足, 返回值为 0 , 否则 err_code.
*/
static int e902_enter_debug( void )
{
	int iret;
	uint32_t temp;

	/**/
	had_null_clock();
	had_null_clock();

	/**/
	iret = had_read_dr32( &temp );
	if ( iret != 0 ) {
		return 1;
	}

	if ( (temp == 0) || (temp == 0xFFFFFFFF) ) {
		return 2;
	}

	/* HSR */
	iret = had_read_dr32_by_index( HREG_HSR, &temp );
	if ( iret != 0 ) {
		return 3;
	}

	/* already debug mode ?? */
	if ( (temp & 0x3) == 0x2 ) {
		return 0;
	}

	/* DR = 1, bit-15  */
	had_write_dr32_by_index( HREG_HCR, 0x8000 );

	/* HSR */
	iret = had_read_dr32_by_index( HREG_HSR, &temp );
	if ( iret != 0 ) {
		return 3;
	}

	/**/
	if ( (temp & 0x3) == 0x2 ) {
		return 0;
	}

	return 4;
}


/*
* 前提条件是处于 debug 模式.
* 使用 DDC 方式来写内存?
*/
static int e902_write_memory( uint32_t addr, int num, uint32_t * ptr )
{
	int i;

	/* DDCEN = 1, bit-20 */
	had_write_dr32_by_index( HREG_HCR, 0x100000 );
	had_write_dr32_by_index( HREG_ADDR, addr );
	had_write_ir8( HREG_DATA );

	/**/
	for ( i=0; i<num; i++ ) {
		had_write_dr32( ptr[i] );
	}

	return 0;
}


/*
* 前提条件是处于 debug 模式.
* 设置某些 gpr 的值, 然后退出 debug 模式, 恢复运行模式.
* 需要关注某个 csr 的值, 保证关闭中断, 保证进入 machine 特权模式.
*/
static int e902_exit_debug( uint32_t epc )
{
	int iret;
	uint32_t temp;

	/**/
	had_write_dr32_by_index( HREG_HCR, 0 );
	had_write_dr32_by_index( HREG_PC, epc );
	had_write_dr32_by_index( 0x60 | HREG_BYPASS, 0 );

	/* check */
	iret = had_read_dr32_by_index( HREG_HSR, &temp );
	if ( iret != 0 ) {
		return 1;
	}

	if ( temp != 0 ) {
		return 2;
	}

	return 0;
}



int had_msg_e902_enter( umsg_t * pmsg )
{
	int iret;

	/**/
	iret = e902_enter_debug();

	/**/
	*(uint32_t *)pmsg->data = (uint32_t)iret;
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}


int had_msg_e902_exit( umsg_t * pmsg )
{
	int iret;
	uint32_t temp;

	/**/
	temp = *(uint32_t *)pmsg->data;
	iret = e902_exit_debug( temp );

	/**/
	*(uint32_t *)pmsg->data = (uint32_t)iret;
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}


int had_msg_e902_write_mem( umsg_t * pmsg )
{
	int iret;
	uint32_t addr;

	if ( pmsg->mlen < 8 ) {
		*(uint32_t *)pmsg->data = (uint32_t)0x101;
		pmsg->mlen = 4;
		multicore_fifo_push_blocking( (intptr_t)pmsg );
		return 0;
	}

	/**/
	addr = *(uint32_t *)pmsg->data;
	iret = e902_write_memory( addr, (pmsg->mlen - 4) / 4, (uint32_t *)(pmsg->data + 4) );

	/**/
	*(uint32_t *)pmsg->data = (uint32_t)iret;
	pmsg->mlen = 4;
	multicore_fifo_push_blocking( (intptr_t)pmsg );		
	return 0;
}




void core1_main( void )
{
	umsg_t * pmsg;
	uint32_t cnt = 0;

	printf("hello, from, %d\n", sio_hw->cpuid );

	/* I2C */
	gpio_pull_up( 0 );
	gpio_pull_up( 1 );
	gpio_set_input_enabled( 0, true );
	gpio_set_input_enabled( 1, true );
	gpio_set_input_hysteresis_enabled( 0, true );
	gpio_set_input_hysteresis_enabled( 1, true );
	gpio_set_slew_rate( 0, GPIO_SLEW_RATE_SLOW );
	gpio_set_slew_rate( 1, GPIO_SLEW_RATE_SLOW );
	gpio_set_function( 0, GPIO_FUNC_I2C );
	gpio_set_function( 1, GPIO_FUNC_I2C );
	

	while ( 1 ) {

		/**/
		pmsg = (umsg_t *)(uintptr_t)multicore_fifo_pop_blocking();
		cnt += 1;

		// printf( "pmsg, %u: %p, %u, %u\n", cnt, pmsg, pmsg->type, pmsg->mlen );
		
		switch ( pmsg->type ) {

		case 0x10:
			p1_test_i2c( pmsg );
			break;

		case 0x11:
			/* echo */
			multicore_fifo_push_blocking( (intptr_t)pmsg );
			break;

		// ws2812b
		case 0x12:
			p1_test_pio( pmsg );
			break;
		
		case 0x13: 
			p1_test_pio_data( pmsg );
			break;

		// e902, had debug.
		case 0x16:
			had_init_pio( pmsg );
			break;

		case 0x17:
			had_msg_null_clock( pmsg );
			break;

		case 0x18:
			had_msg_write_ir8( pmsg );
			break;

		case 0x19:
			had_msg_write_dr32( pmsg );
			break;

		case 0x1A:
			had_msg_read_dr32( pmsg );
			break;

		case 0x1B:
			had_msg_write_dr32_by_index(pmsg);
			break;

		case 0x1C:
			had_msg_read_dr32_by_index(pmsg);
			break;

		case 0x1D:
			had_msg_e902_enter( pmsg );
			break;

		case 0x1E:
			had_msg_e902_exit( pmsg );
			break;

		case 0x1F:
			had_msg_e902_write_mem( pmsg );
			break;

		default:
			umsg_free( pmsg );
			break;
		}

		// test, echo..
		// multicore_fifo_push_blocking( (uint32_t)(uintptr_t)pmsg );
	}
}


